#include <btl/extra/Gui/ColourMap.hpp>

namespace {

   // order of data is BOUNDARY, RED, GREEN, BLUE, ALPHA

   const float COLOURDATA_HSV[][5] = {
      {0.0f/6.0f, 1.0f, 0.0f, 0.0f, 1.0f},
      {1.0f/6.0f, 1.0f, 1.0f, 0.0f, 1.0f},
      {2.0f/6.0f, 0.0f, 1.0f, 0.0f, 1.0f},
      {3.0f/6.0f, 0.0f, 1.0f, 1.0f, 1.0f},
      {4.0f/6.0f, 0.0f, 0.0f, 1.0f, 1.0f},
      {5.0f/6.0f, 1.0f, 0.0f, 1.0f, 1.0f},
      {6.0f/6.0f, 1.0f, 0.0f, 0.0f, 1.0f}
   };
   const int COLOURDATA_HSV_LEN = sizeof(COLOURDATA_HSV) / sizeof(COLOURDATA_HSV[0]);

   const float COLOURDATA_JET[][5] = {
      {0.0f/4.0f, 0.0f, 0.0f, 0.5f, 1.0f},
      {0.5f/4.0f, 0.0f, 0.0f, 1.0f, 1.0f},
      {1.5f/4.0f, 0.0f, 1.0f, 1.0f, 1.0f},
      {2.5f/4.0f, 1.0f, 1.0f, 0.0f, 1.0f},
      {3.5f/4.0f, 1.0f, 0.0f, 0.0f, 1.0f},
      {4.0f/4.0f, 0.5f, 0.0f, 0.0f, 1.0f}
   };
   const int COLOURDATA_JET_LEN = sizeof(COLOURDATA_JET) / sizeof(COLOURDATA_JET[0]);

   const float COLOURDATA_GREY[][5] = {
      {0.0f, 0.3f, 0.3f, 0.3f, 1.0f},
      {1.0f, 1.0f, 1.0f, 1.0f, 1.0f}
   };
   const int COLOURDATA_GREY_LEN = sizeof(COLOURDATA_GREY) / sizeof(COLOURDATA_GREY[0]);

   const float COLOURDATA_KWHITE[][5] = {
      {0.0f, 1.0f, 1.0f, 1.0f, 1.0f},
      {1.0f, 1.0f, 1.0f, 1.0f, 1.0f}
   };
   const int COLOURDATA_KWHITE_LEN = sizeof(COLOURDATA_KWHITE) / sizeof(COLOURDATA_KWHITE[0]);
}

namespace btl
{
namespace extra
{
namespace gui
{

const ColourMap ColourMap::HSV= ColourMap(COLOURDATA_HSV, COLOURDATA_HSV_LEN);
const ColourMap ColourMap::Jet = ColourMap(COLOURDATA_JET, COLOURDATA_JET_LEN);
const ColourMap ColourMap::Grey = ColourMap(COLOURDATA_GREY, COLOURDATA_GREY_LEN);
const ColourMap ColourMap::ConstantWhite = ColourMap(COLOURDATA_KWHITE, COLOURDATA_KWHITE_LEN);

ColourMap::ColourMap()
{
}

ColourMap::ColourMap(const float data[][5], int count)
{
   assert(count >= 2);

   _stageBoundaries.resize(count);
   _stages.resize(4, count);

   for (int i = 0; i < count; ++i) {
      _stageBoundaries(i) = data[i][0];
      _stages.col(i) = Eigen::Vector4f(data[i][1], data[i][2], data[i][3], data[i][4]);
   }

   // clamp ends to the ends
   _stageBoundaries(0) = 0.0f;
   _stageBoundaries(count-1) = 1.0f;
}

Eigen::Vector4f ColourMap::at(double x) const
{
   const int N = _stageBoundaries.size();
   assert(N >= 2);

   int i = 0;
   while ((i < N-1) && (_stageBoundaries(i+1) < x))
      ++i;

   float a = _stageBoundaries(i);
   float b = _stageBoundaries(i+1);
   float alpha = (float(x) - a) / (b - a);

   return (_stages.col(i) + alpha*(_stages.col(i+1) - _stages.col(i)));
}

} // namespace gui
} // namespace extra
} // namespace btl
